using System;
using System.Diagnostics;
using System.Runtime.InteropServices;
using System.Text;

// NOTE.  The (x,y,z) coordinate system is assumed to be right-handed.
// Coordinate axis rotation matrices are of the form
//   RX =    1       0       0
//           0     cos(t) -sin(t)
//           0     sin(t)  cos(t)
// where t > 0 indicates a counterclockwise rotation in the yz-plane
//   RY =  cos(t)    0     sin(t)
//           0       1       0
//        -sin(t)    0     cos(t)
// where t > 0 indicates a counterclockwise rotation in the zx-plane
//   RZ =  cos(t) -sin(t)    0
//         sin(t)  cos(t)    0
//           0       0       1
// where t > 0 indicates a counterclockwise rotation in the xy-plane.

namespace RSEngine.Math
{
    /// <summary>
    /// A 3x3 matrix which can represent rotations around axes.
    /// 
    /// Matrix values structure :
    ///	| m[0][0]  m[1][0]  m[2][0]  |
    ///	| m[0][1]  m[1][1]  m[2][1]  |
    ///	| m[0][2]  m[1][2]  m[2][2]  |
    /// </summary>
    [StructLayout(LayoutKind.Sequential)]
    public struct Matrix3
    {

        #region Variables

        public float[] Values;

        public static readonly Matrix3 Identity = new Matrix3(
            1, 0, 0,
            0, 1, 0,
            0, 0, 1);

        public static readonly Matrix3 Zero = new Matrix3(
            0, 0, 0,
            0, 0, 0,
            0, 0, 0);

        #endregion

        #region Constructors

        /// <summary>
        ///	Creates a new Matrix3 with all the specified parameters.
        /// </summary>
        public Matrix3(
            float m00, float m10, float m20,
            float m01, float m11, float m21,
            float m02, float m12, float m22)
        {
            Values = new float[3 * 3];

            this.M00 = m00;
            this.M10 = m10;
            this.M20 = m20;
            this.M01 = m01;
            this.M11 = m11;
            this.M21 = m21;
            this.M02 = m02;
            this.M12 = m12;
            this.M22 = m22;
        }

        public Matrix3(Matrix4 right)
        {
            Values = new float[3 * 3];

            M00 = right.M00;
            M01 = right.M01;
            M02 = right.M02;
            M10 = right.M10;
            M11 = right.M11;
            M12 = right.M12;
            M20 = right.M20;
            M21 = right.M21;
            M22 = right.M22;
        }

        #endregion

        #region Properties

        public float Determinant
        {
            get
            {
                float cofactor00 = Values[4] * Values[8] - Values[7] * Values[5];
                float cofactor10 = Values[7] * Values[2] - Values[1] * Values[8];
                float cofactor20 = Values[1] * Values[5] - Values[4] * Values[2];

                float result =
                    Values[0] * cofactor00 +
                    Values[3] * cofactor10 +
                    Values[6] * cofactor20;

                return result;
            }
        }

        public float M00
        {
            get
            {
                return Values[0];
            }
            set
            {
                Values[0] = value;
            }
        }

        public float M10
        {
            get
            {
                return Values[1];
            }
            set
            {
                Values[1] = value;
            }
        }

        public float M20
        {
            get
            {
                return Values[2];
            }
            set
            {
                Values[2] = value;
            }
        }

        public float M01
        {
            get
            {
                return Values[3];
            }
            set
            {
                Values[3] = value;
            }
        }

        public float M11
        {
            get
            {
                return Values[4];
            }
            set
            {
                Values[4] = value;
            }
        }

        public float M21
        {
            get
            {
                return Values[5];
            }
            set
            {
                Values[5] = value;
            }
        }

        public float M02
        {
            get
            {
                return Values[6];
            }
            set
            {
                Values[6] = value;
            }
        }

        public float M12
        {
            get
            {
                return Values[7];
            }
            set
            {
                Values[7] = value;
            }
        }

        public float M22
        {
            get
            {
                return Values[8];
            }
            set
            {
                Values[8] = value;
            }
        }

        #endregion

        #region Features

        /// <summary>
        /// Swap the rows of the matrix with the columns.
        /// </summary>
        /// <returns>A transposed Matrix.</returns>
        public Matrix3 Transpose()
        {
            return new Matrix3(
                Values[0], Values[1], Values[2],
                Values[3], Values[4], Values[5],
                Values[6], Values[7], Values[8]);
        }

        /// <summary>
        /// Constructs this Matrix from 3 euler angles, in degrees.
        /// </summary>
        /// <param name="yaw"></param>
        /// <param name="pitch"></param>
        /// <param name="roll"></param>
        public void FromEulerAnglesXYZ(float yaw, float pitch, float roll)
        {
            float cos = (float)System.Math.Cos(yaw);
            float sin = (float)System.Math.Sin(yaw);
            Matrix3 xMat = new Matrix3(1, 0, 0, 0, cos, -sin, 0, sin, cos);

            cos = (float)System.Math.Cos(pitch);
            sin = (float)System.Math.Sin(pitch);
            Matrix3 yMat = new Matrix3(cos, 0, sin, 0, 1, 0, -sin, 0, cos);

            cos = (float)System.Math.Cos(roll);
            sin = (float)System.Math.Sin(roll);
            Matrix3 zMat = new Matrix3(cos, -sin, 0, sin, cos, 0, 0, 0, 1);

            this = xMat * (yMat * zMat);
        }

        #endregion

        #region Operator overloads

        /// <summary>
        /// Multiply (concatenate) two Matrix3 instances together.
        /// </summary>
        public static Matrix3 operator *(Matrix3 left, Matrix3 right)
        {

            Matrix3 result = new Matrix3();

            result.Values[0] = left.Values[0] * right.Values[0] + left.Values[3] * right.Values[1] + left.Values[6] * right.Values[2];
            result.Values[3] = left.Values[0] * right.Values[3] + left.Values[3] * right.Values[4] + left.Values[6] * right.Values[5];
            result.Values[6] = left.Values[0] * right.Values[6] + left.Values[3] * right.Values[7] + left.Values[6] * right.Values[8];

            result.Values[1] = left.Values[1] * right.Values[0] + left.Values[4] * right.Values[1] + left.Values[7] * right.Values[2];
            result.Values[4] = left.Values[1] * right.Values[3] + left.Values[4] * right.Values[4] + left.Values[7] * right.Values[5];
            result.Values[7] = left.Values[1] * right.Values[6] + left.Values[4] * right.Values[7] + left.Values[7] * right.Values[8];

            result.Values[2] = left.Values[2] * right.Values[0] + left.Values[5] * right.Values[1] + left.Values[8] * right.Values[2];
            result.Values[5] = left.Values[2] * right.Values[3] + left.Values[5] * right.Values[4] + left.Values[8] * right.Values[5];
            result.Values[8] = left.Values[2] * right.Values[6] + left.Values[5] * right.Values[7] + left.Values[8] * right.Values[8];

            return result;
        }

        /// <summary>
        ///	vector * matrix [1x3 * 3x3 = 1x3]
        /// </summary>
        public static Vector3 operator *(Vector3 vector, Matrix3 matrix)
        {
            Vector3 product = new Vector3();

            product.x = matrix.Values[0] * vector.x + matrix.Values[3] * vector.y + matrix.Values[6] * vector.z;
            product.y = matrix.Values[1] * vector.x + matrix.Values[4] * vector.y + matrix.Values[7] * vector.z;
            product.z = matrix.Values[2] * vector.x + matrix.Values[5] * vector.y + matrix.Values[8] * vector.z;

            return product;
        }

        /// <summary>
        ///	matrix * vector [3x3 * 3x1 = 3x1]
        /// </summary>
        public static Vector3 operator *(Matrix3 matrix, Vector3 vector)
        {
            Vector3 product = new Vector3();

            product.x = matrix.Values[0] * vector.x + matrix.Values[3] * vector.y + matrix.Values[6] * vector.z;
            product.y = matrix.Values[1] * vector.x + matrix.Values[4] * vector.y + matrix.Values[7] * vector.z;
            product.z = matrix.Values[2] * vector.x + matrix.Values[5] * vector.y + matrix.Values[8] * vector.z;

            return product;
        }

        /// <summary>
        /// Multiplies all the items in the Matrix3 by a scalar value.
        /// </summary>
        public static Matrix3 operator *(Matrix3 matrix, float scalar)
        {
            Matrix3 result = new Matrix3();

            result.Values[0] = matrix.Values[0] * scalar;
            result.Values[3] = matrix.Values[3] * scalar;
            result.Values[6] = matrix.Values[6] * scalar;
            result.Values[1] = matrix.Values[1] * scalar;
            result.Values[4] = matrix.Values[4] * scalar;
            result.Values[7] = matrix.Values[7] * scalar;
            result.Values[2] = matrix.Values[2] * scalar;
            result.Values[5] = matrix.Values[5] * scalar;
            result.Values[8] = matrix.Values[8] * scalar;

            return result;
        }

        /// <summary>
        /// Multiplies all the items in the Matrix3 by a scalar value.
        /// </summary>
        public static Matrix3 operator *(float scalar, Matrix3 matrix)
        {
            Matrix3 result = new Matrix3();

            result.Values[0] = matrix.Values[0] * scalar;
            result.Values[3] = matrix.Values[3] * scalar;
            result.Values[6] = matrix.Values[6] * scalar;
            result.Values[1] = matrix.Values[1] * scalar;
            result.Values[4] = matrix.Values[4] * scalar;
            result.Values[7] = matrix.Values[7] * scalar;
            result.Values[2] = matrix.Values[2] * scalar;
            result.Values[5] = matrix.Values[5] * scalar;
            result.Values[8] = matrix.Values[8] * scalar;

            return result;
        }

        /// <summary>
        ///	Used to add two matrices together.
        /// </summary>
        /// <param name="left"></param>
        /// <param name="right"></param>
        /// <returns></returns>
        public static Matrix3 operator +(Matrix3 left, Matrix3 right)
        {
            Matrix3 result = new Matrix3();

            for (int i = 0; i < 9; i++)
                result.Values[i] = left.Values[i] + right.Values[i];

            return result;
        }

        /// <summary>
        ///	Used to subtract two matrices.
        /// </summary>
        public static Matrix3 operator -(Matrix3 left, Matrix3 right)
        {
            Matrix3 result = new Matrix3();

            for (int i = 0; i < 9; i++)
                result.Values[i] = left.Values[i] - right.Values[i];

            return result;
        }

        /// <summary>
        /// Negates all the items in the Matrix.
        /// </summary>
        public static Matrix3 operator -(Matrix3 matrix)
        {
            Matrix3 result = new Matrix3();

            result.Values[0] = -matrix.Values[0];
            result.Values[3] = -matrix.Values[3];
            result.Values[6] = -matrix.Values[6];
            result.Values[1] = -matrix.Values[1];
            result.Values[4] = -matrix.Values[4];
            result.Values[7] = -matrix.Values[7];
            result.Values[2] = -matrix.Values[2];
            result.Values[5] = -matrix.Values[5];
            result.Values[8] = -matrix.Values[8];

            return result;
        }

        /// <summary>
        /// Test two matrices for (value) equality
        /// </summary>
        public static bool operator ==(Matrix3 left, Matrix3 right)
        {
            if (
                left.Values[0] == right.Values[0] && left.Values[3] == right.Values[3] && left.Values[6] == right.Values[6] &&
                left.Values[1] == right.Values[1] && left.Values[4] == right.Values[4] && left.Values[7] == right.Values[7] &&
                left.Values[2] == right.Values[2] && left.Values[5] == right.Values[5] && left.Values[8] == right.Values[8])
                return true;

            return false;
        }

        public static bool operator !=(Matrix3 left, Matrix3 right)
        {
            return !(left == right);
        }

        /// <summary>
        /// Indexer for accessing the matrix like a 2d array (i.e. matrix[2,3]).
        /// </summary>
        public float this[int col, int row]
        {
            get
            {
                unsafe
                {
                    fixed (float* pM = &Values[0])
                        return *(pM + ((3 * row) + col));
                }
            }
            set
            {
                unsafe
                {
                    fixed (float* pM = &Values[0])
                        *(pM + ((3 * row) + col)) = value;
                }
            }
        }

        #endregion

        #region Clone

        public Matrix3 Clone()
        {
            Matrix3 clone = new Matrix3();
            clone.Values = new float[3 * 3];
            clone.Values[0] = Values[0];
            clone.Values[1] = Values[1];
            clone.Values[2] = Values[2];
            clone.Values[3] = Values[3];
            clone.Values[4] = Values[4];
            clone.Values[5] = Values[5];
            clone.Values[6] = Values[6];
            clone.Values[7] = Values[7];
            clone.Values[8] = Values[8];
            return clone;
        }

        #endregion

        #region Object overloads

        /// <summary>
        ///		Overrides the Object.ToString() method to provide a text representation of 
        ///		a Matrix4.
        /// </summary>
        /// <returns>A string representation of a vector3.</returns>
        public override string ToString()
        {
            StringBuilder builder = new StringBuilder();

            builder.AppendFormat(" | {0} {1} {2} |\n", Values[0], Values[3], Values[6]);
            builder.AppendFormat(" | {0} {1} {2} |\n", Values[1], Values[4], Values[7]);
            builder.AppendFormat(" | {0} {1} {2} |", Values[2], Values[5], Values[8]);

            return builder.ToString();
        }

        /// <summary>
        ///		Provides a unique hash code based on the member variables of this
        ///		class.  This should be done because the equality operators (==, !=)
        ///		have been overriden by this class.
        ///		<p/>
        ///		The standard implementation is a simple XOR operation between all local
        ///		member variables.
        /// </summary>
        /// <returns></returns>
        public override int GetHashCode()
        {
            int hashCode = 0;

            unsafe
            {
                fixed (float* pM = &Values[0])
                {
                    for (int i = 0; i < 9; i++)
                        hashCode ^= (int)(*(pM + i));
                }

                return hashCode;
            }
        }

        /// <summary>
        ///		Compares this Matrix to another object.  This should be done because the 
        ///		equality operators (==, !=) have been overriden by this class.
        /// </summary>
        /// <param name="obj"></param>
        /// <returns></returns>
        public override bool Equals(object obj)
        {
            return obj is Matrix3 && this == (Matrix3)obj;
        }

        #endregion

    }
}
